#include "Flybox.h"
#include <stdlib.h>
#include <iostream>
using namespace std;
#include <boost/utility/value_init.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;


CFlybox::CFlybox()
{
    _sBGData = boost::initialized_value; // initialize all struct members to 0
    _bFlyOpen = false;
}

CFlybox::~CFlybox()
{
}

bool CFlybox::InitFlybox()
{
    int st;
    if (setup_lv()!=-1)
    {
        if ((st = w_lv(_sBGData.sp_fd, "o")) < 0) /* startup the Flybox (request data) */
            return false; // writing failed: Flybox not successfully opened

        // Flybox successfully started
        _bFlyOpen = true;
        return true;
    }
    return false;
}

int CFlybox::setup_lv()
{
    int st;
   
/*
 * We only ever need the first 3 analog inputs and one digital, the less we request, the faster we can sample
 */
 
    _sBGData.analog_in = 0;
    _sBGData.analog_in = AIC1 | AIC2 | AIC3;

    _sBGData.dig_in = 0;
    _sBGData.dig_in = DIC2; //trigger

/*
 *  Set the baud rate
 */
    _sBGData.baud    = BAUD1152; // DN: higher baud rate support lower read interval - tested 115.2K successfully at 4 ms
 

    st = open_lv(&_sBGData, FB_COMPORT, FB_NOBLOCK);
    if (st < 0)
    {
        printf("Unable to open port\n");
        return -1;
    }
 
/*
 *  Send the init string
 */
    st = init_lv(&_sBGData);
    if ( st < 0 )
    {
        printf("Invalid setup requested.  Bye\n");
        return -1;
    }

    // completed successfully
    return 0;
}
void CFlybox::rf_quit()
{
    close_lv(&_sBGData);
    _bFlyOpen = false;
}

bool CFlybox::ReadFly()
{
    if (_bFlyOpen == false)
    {
        cout << "Flybox: not initialized" << endl;
        return false;
    }

    // first read request (w_lv(bgdata.sp_fd, "o")) happened in InitFlybox(), so there should be data waiting for us
    if (r_lv(&_sBGData)) // read ok when r_lv() returns 0
    {
        throw exception("Flybox: trying to read data too quick, take a longer break");
        return false;
    }

    // request data for next read
    if(w_lv(_sBGData.sp_fd, "o") < 1) // write ok when w_lv returns >=1 (1 expected as one character written)
    {
        throw exception("Flybox: data request failed");
        return false;
    }

    // completed successfully
    return true;
}

std::vector<float> CFlybox::GetState()
{
    std::vector<float> _vReturn;
    
    if (!ReadFly())
        _vReturn.assign(3,99.f);
    else
        _vReturn += -_sBGData.ain[0],_sBGData.ain[1],_sBGData.ain[2];  // 0 horizontal, 1 vertical, 2 twist

    return _vReturn;
}

int CFlybox::IsTriggerDepressed()
{
    if (!ReadFly())
        return 99;
    return _sBGData.din[1];  // 1 if depressed, 0 if not
}